#include "stdafx.h"
#include "JugadorEjemplo.h"

JugadorEjemplo::JugadorEjemplo()
{
	masCerca = false;
}

JugadorEjemplo::~JugadorEjemplo()
{
}


void JugadorEjemplo::OnPlay()
{
	double x, y;
	double vl, vr;
	double xo,yo;

	if (masCerca)
	{
		x = env->predictedBall.pos.x;
		y = env->predictedBall.pos.y;
		if ( y > FTOP - 2.5 ) 
			 y = FTOP - 2.5;
		if ( y < FBOT + 2.5 ) 
			 y = FBOT + 2.5;
		if ( x > FRIGHTX - 3 ) 
			 x = FRIGHTX - 3;
		if ( x < FLEFTX + 3 ) 
			 x = FLEFTX + 3;

		xo = robot->pos.x;
		yo = robot->pos.y;
		double r = robot->rotation;
		while (r > 180)
			r -= 360;
		while (r < -180)
			r += 360;

		double ang, angleDiff;

		ang = ObtenerAngulo(xo,yo,x,y);

		angleDiff = ang - r;
		// Normalizo a -180 a 180
		if (angleDiff > 180)
			angleDiff -= 360;
		if (angleDiff < -180)
			angleDiff += 360;

		if (fabs(angleDiff) < 15)
		{
			// Estoy apuntando mas o menos hacia la pelota => voy hacia adelante
			vl = MaxVel;
			vr = MaxVel;
		}
		else
		{
			// Ajusto orientacion
			vl = -angleDiff * ATENUAR;
			vr = angleDiff * ATENUAR;
		}
	}
	else
	{
		// Si no es el mas cercano se queda quieto
		vl = 0;
		vr = 0;
	}

	robot->velocityLeft = vl;
	robot->velocityRight = vr;
}
